package autonomous;

import java.util.ArrayList;

import processing.core.PApplet;
import processing.core.PVector;

public class Path {

	// A Path is an arraylist of points (PVector objects)
	ArrayList points;
	// A path has a radius, i.e how far is it ok for the boid to wander off
	float radius;
	PApplet parent; //

	public Path(PApplet p) {
		parent = p;

		// Arbitrary radius of 20
		radius = 20;
		points = new ArrayList();
	}

	// Add a point to the path
	public void addPoint(float x, float y) {
		PVector point = new PVector(x, y);
		points.add(point);
	}

	// Draw the path
	public void display() {

		// Draw the radius as thick lines and circles
		if (true) {
			// if (debug) {
			// Draw end points
			for (int i = 0; i < points.size(); i++) {
				PVector point = (PVector) points.get(i);
				parent.fill(175);
				parent.noStroke();
				parent.ellipse(point.x, point.y, radius * 2, radius * 2);
			}

			// Draw Polygon around path
			for (int i = 0; i < points.size() - 1; i++) {
				PVector start = (PVector) points.get(i);
				PVector end = (PVector) points.get(i + 1);
				PVector line = PVector.sub(end, start);
				PVector normal = new PVector(line.y, -line.x);
				normal.normalize();
				normal.mult(radius);

				// Polygon has four vertices
				PVector a = PVector.add(start, normal);
				PVector b = PVector.add(end, normal);
				PVector c = PVector.sub(end, normal);
				PVector d = PVector.sub(start, normal);

				parent.fill(175);
				parent.noStroke();
				parent.beginShape();
				parent.vertex(a.x, a.y);
				parent.vertex(b.x, b.y);
				parent.vertex(c.x, c.y);
				parent.vertex(d.x, d.y);
				parent.endShape();
			}
		}

		// Draw Regular Line
		parent.stroke(0);
		parent.noFill();
		parent.beginShape();
		for (int i = 0; i < points.size(); i++) {
			PVector loc = (PVector) points.get(i);
			parent.vertex(loc.x, loc.y);
		}
		parent.endShape();

	}

}